module pid_rx(
    input   wire            sys_clk ,
    input   wire            reset_n ,
    input   wire            en_flag , // 开始校准
    input   wire            real_en , // 是否需要校准
    input   wire            down_en , // 特殊处理，下抓的时候保持上身不变，只动下半身
    input   wire            uart_rx ,

    input   wire    [11:0]  in_time ,
    input   wire    [11:0]  angle_1 , // 目标值
    input   wire    [11:0]  angle_2 , // 目标值
    input   wire    [11:0]  angle_3 , // 目标值
    input   wire    [11:0]  angle_4 , // 目标值
    input   wire    [11:0]  angle_5 , // 目标值
    input   wire    [11:0]  angle_6 , // 目标值

    output  reg             real_start   , // 校准模式
    output  reg             real_send    , // 发送信号（可以是校准信号，也可以是发送控制信号）
    output  wire            real_flag    , // 每次回读完成的信号

    output  reg     [11:0]  angle_1_real , // 小臂校正值
    output  reg     [11:0]  angle_2_real , // 小臂校正值
    output  reg     [11:0]  angle_3_real , // 小臂校正值
    output  reg     [11:0]  angle_4_real , // 小臂校正值
    output  reg     [11:0]  angle_5_real , // 小臂校正值
    output  reg     [11:0]  angle_6_real , // 小臂校正值
    output  wire    [11:0]  out_time     ,

    output  reg             real_finish    // 校准完毕
);
// RX
wire    [3:0]           real_id    ;
wire    [11:0]          real_data  ;

parameter   S1 = 8'b0000_0001;
parameter   S2 = 8'b0000_0010;
parameter   S3 = 8'b0000_0100;
parameter   S4 = 8'b0000_1000;
parameter   S5 = 8'b0001_0000;
parameter   S6 = 8'b0010_0000;
parameter   S7 = 8'b0100_0000;
parameter   S8 = 8'b1000_0000;

// reg     [11:0]  angle_1_real ; // 小臂校正值
// reg     [11:0]  angle_2_real ; // 小臂校正值
// reg     [11:0]  angle_3_real ; // 小臂校正值
// reg     [11:0]  angle_4_real ; // 小臂校正值
// reg     [11:0]  angle_5_real ; // 小臂校正值
// reg     [11:0]  angle_6_real ; // 小臂校正值

/*
开始校准
stap1: 控制机械臂移动
stap2: 读取机械臂校准值(连续)
stap3: 接收并处理机械臂偏差(连续)
stap4: 继续控制机械臂移动
...
stapX: 校准完成，进行下一个动作的执行
*/

reg [7:0]       pid_state    ; // 状态机
reg [7:0]       rx_real_state; // 校准次数
reg             over_flag    ;
// 计数值
reg             cnt_reg  ;
reg     [27:0]  cnt      ;
wire    [27:0]  cnt_max  ;
// 记录状态
reg     [7:0]   rx_state ;
reg             reread_flag;
// 计数最大值，与控制机械臂移动的时间成正比
assign cnt_max      = 28'd55_000 * out_time; // 50_000_0 等待1.2秒(可能会消耗资源) 60_000
assign out_time     = (rx_real_state == S1) ? in_time : 12'd100; // 除了第一次，其他都是快速移动

// PID校准全流程
always @(posedge sys_clk or negedge reset_n) begin
    if(!reset_n) begin
        real_start  <= 1'b0;
        real_send   <= 1'b0;
        real_finish <= 1'b0;
        over_flag   <= 1'b0;
        cnt         <= 1'b0;
        cnt_reg     <= 1'b0;
        reread_flag <= 1'b0;
        pid_state   <= S1;
        rx_real_state <= S1;
    // 计数模块,等待机械臂运转完毕
    end else if(cnt == cnt_max) begin
        cnt       <= 0 ;
        cnt_reg   <= 0 ; // 停止计数
        over_flag <= 1 ; // 发送驱动信号
    end else if(cnt_reg) begin // 计数ing
        cnt <= cnt + 1'b1;
        real_send  <= 0 ; // 脉冲
    // 判断一次校准是否结束
    end else if(rx_state == S5) begin // 已经回读完毕，可以进行下一步操作
        over_flag  <= 1'b1;
    end else if(en_flag || over_flag) begin
        over_flag <= 0;
        casex(pid_state) // 机械臂状态
        S1:begin // 控制机械臂移动到初始位置（校准前）
            real_start <= 0 ; // 校准模式关
            real_send  <= 1 ; // 发送数据（机械臂控制数据）
            cnt_reg    <= 1 ; // 开始计时
            rx_real_state <= S1;

            if(real_en) pid_state  <= S2; // 如果需要校准，走正常流程
            else pid_state  <= S4;
        end
        S2:begin // 发送回读指令(不需要计时)
            real_start <= 1'b1; // 校准模式开
            real_send  <= 1'b1; // 发送数据（机械臂回读指令）
            reread_flag<= 1'b1; // 回读初始化（下面）

            pid_state  <= S3;
        end
        S3:begin // 进行二次校准
            if(rx_real_state == S4) begin // 已经校准8次，可以停手
                pid_state  <= S1;
                real_finish <= 1; // 全部校准完成
            end else begin // 控制机械臂到指定区域（速度快），继续回读
                rx_real_state <= rx_real_state << 1; // 校准次数 +1
                real_start    <= 0; // 校准模式关
                real_send     <= 1; // 发送数据（机械臂控制数据）
                // out_time   <= 12'd100;
                cnt_reg    <= 1 ; // 开始计时

                pid_state  <= S2;
            end
        end
        S4:begin
            pid_state  <= S1;
            real_finish <= 1; // 全部校准完成
        end
        default: pid_state <= S1;
        endcase
    end else begin
        real_start  <= real_start;
        real_send   <= 1'b0;
        real_finish <= 1'b0;
        over_flag   <= 1'b0; //
        cnt         <= 1'b0; //
        cnt_reg     <= 1'b0; //
        reread_flag <= 1'b0;
        pid_state   <= pid_state;
        rx_real_state <= rx_real_state;
    end
end

// 回读处理信息
always @(posedge sys_clk or negedge reset_n) begin
    if(!reset_n) begin
        angle_1_real <= 0;
        angle_2_real <= 0;
        angle_3_real <= 0;
        angle_4_real <= 0;
        angle_5_real <= 0;
        angle_6_real <= 0;
        rx_state     <= S1;
    end else if(en_flag) begin // 开始校准，引入初始值
        if(!down_en) begin // 如果是下抓，就不更新上半身
            angle_2_real <= angle_2;
            angle_3_real <= angle_3;
            angle_4_real <= angle_4;
            angle_6_real <= angle_6;
        end
        // angle_6_real <= angle_6;
        angle_1_real <= angle_1;
        angle_5_real <= angle_5;
    end else if(reread_flag) begin //初始化回读数目
        rx_state     <= S1;
    end else if(real_flag) begin // 读取完毕（回读模式）
        rx_state <= rx_state << 1;
        casex(real_id) // 判断误差
        4'd0:begin
            angle_6_real <= angle_6_real;
            // if(angle_6 > real_data) begin
            //     angle_6_real <= angle_6_real + ((angle_6 - real_data) >> 2);
            // end else begin
            //     angle_6_real <= angle_6_real - ((real_data - angle_6) >> 2);
            // end
        end
        4'd1:begin
            angle_5_real <= angle_5_real;
        end
        4'd2:begin
            if(angle_4 > real_data) begin
                angle_4_real <= angle_4_real + ((angle_4 - real_data) >> 2);
            end else begin
                angle_4_real <= angle_4_real - ((real_data - angle_4) >> 2);
            end
        end
        4'd3:begin
            if(angle_3 > real_data) begin
                angle_3_real <= angle_3_real + ((angle_3 - real_data) >> 2);
            end else begin
                angle_3_real <= angle_3_real - ((real_data - angle_3) >> 2);
            end
        end
        4'd4:begin
            if(angle_2 > real_data) begin
                angle_2_real <= angle_2_real + ((angle_2 - real_data) >> 2);
            end else begin
                angle_2_real <= angle_2_real - ((real_data - angle_2) >> 2);
            end
        end
        default:begin
            angle_1_real <= angle_1_real;
            angle_2_real <= angle_2_real;
            angle_3_real <= angle_3_real;
            angle_4_real <= angle_4_real;
            angle_5_real <= angle_5_real;
            angle_6_real <= angle_6_real;
        end
        endcase
    end else begin
        angle_1_real <= angle_1_real;
        angle_2_real <= angle_2_real;
        angle_3_real <= angle_3_real;
        angle_4_real <= angle_4_real;
        angle_5_real <= angle_5_real;
        angle_6_real <= angle_6_real;
        rx_state     <= rx_state    ;
    end
end

// 串口读取数据
rx_data rx_data_inst
(
    .sys_clk    (sys_clk  ),
    .reset_n    (reset_n  ),
    .uart_rx    (uart_rx  ),
    .real_id    (real_id  ),   // 读取的ID
    .real_data  (real_data),   // 读取的数值
    .real_flag  (real_flag)    // 读取完毕
);
endmodule